/**
 * @file	IIRFilter.cpp
 * @author  Thomas Fu <thomas.ks.fu@gmail.com>
 * @version 1.0
 *
 * @section LICENSE
 *
 * This program is propety of The SOSS and may not be used by any 
 * individual for any purpose at any time without express permission from 
 * Mike Casey, Thomas Fu, Chris Hairfield, Kyle Lamson, Ben Treweek, or Cliff 
 * Warren.
 *
 * @brief
 * This class is a representation of a digital, infinite impulse response (IIR)
 * filter. 
 *
 * @class IIRFilter
 */

#include "IIRFilter.h"
#include <assert.h>
#include <cstdlib>
#include <iostream>
#include <vector>

/** 180 Degrees (to 26 digits) */
#define PI 3.1415926535897932384626433

/** Maximum number of poles allowed in IIR Filters (this can be changed) */
#define MAX_FILTER_ORDER 100

/*
 * Constructor for the IIR Filter class.
 * @param num A vector of doubles containing the coefficients of the 
 * polynomial numerator of the z-domain transfer function of the IIR filter.
 * 
 * @param den A vector of doubles containing the coefficients of the 
 * polynomial denominator of the z-domain transfer function of the IIR 
 * filter. 
 */
IIRFilter::IIRFilter( vector<double> num, vector<double> den )
{
  this->num = num;
  this->den = den;
	
	assert(num.size() == den.size());
  this->order = num.size() - 1;
	
	xHistory = (double*)malloc(sizeof(double) * order);
	yHistory = (double*)malloc(sizeof(double) * order);
	
  for(int i = 0; i < order; i++)
	{
		xHistory[i] = 0;
		yHistory[i] = 0;
	}
	
  oldestBufferEntry = order - 1;
}

// Destructor for the IIR Filter
IIRFilter::~IIRFilter()
{
	//free(xHistory);
	//free(yHistory);
}

/*
 * Computes filtered value of the given data point based on the values of 
 * the filter's transfer function as well as a time history maintained
 * internally by the filter. This means that tickFilter must be called on
 * the samples in temporal order.
 *
 * @param val The next value in the signal to be filtered
 *
 * @return The filtered value corresponding to the unfiltered parameter 
 * passed into the function
 */
double IIRFilter::tickFilter(double val)
{
  double numSum = num[0]  * val;
  double denSum = 0;
	
  for(int i = 0; i < order; i++)
	{
		numSum += num[i+1] * xHistory[(oldestBufferEntry+1 + i) % order]; 
		denSum += den[i+1] * yHistory[(oldestBufferEntry+1 + i) % order];
	}
	
  xHistory[oldestBufferEntry] = val;
  yHistory[oldestBufferEntry] = 1/den[0] * (numSum - denSum);
  oldestBufferEntry = (oldestBufferEntry+1) % order;
	
  return yHistory[oldestBufferEntry];
}

/*
 * Computes the coefficients of the numerator and denominator polynomials 
 * for the discrete-time (z-domain) transfer function for a Butterworth 
 * filter of arbitrary order.
 *
 * @param order The number of poles to be used in the filter (the greater 
 * the number of poles, the sharper the filter response).
 *
 * @param w_c The cutoff frequency of the Butterworth filter
 *
 * @param w_s The sampling frequency of the signal being filtered by the 
 * Butterworth filter
 *
 * @param type A value specifying the type of filter to be implemented
 *
 * @param num An array of doubles containing the coefficients of the 
 * polynomial numerator of the transfer function. Space for this array must
 * be allocated before this function is called
 *
 * @param den An array of doubles containing the coefficients of the 
 * polynomial denominator of the transfer function. Space for this array
 * must be allocated before this function is called
 */
void IIRFilter::ButterworthFilter(int order, double w_c, double w_s,
																	FilterType type, double *num, double *den)
{
	
	// Compute some variables
	double w_n = w_c / w_s; 
	double fs = 2;
	double u = 2 * fs * tan(PI * w_n / fs);
	
	// Generate a low-pass filter estimate
	switch(type)
	{
		case LOWPASS: case HIGHPASS: w_n = u; break;
		default: break;
	}
	
	// Generate an equivalent analog low-pass filter 
	complex<double> poles[order];
	
	for(int i = 1; i <= order-1; i+=2)
	{
		poles[i-1] = complex<double> (cos(PI * i / (2 * order) + PI / 2), 
																	sin(PI * i / (2 * order) + PI / 2));
	}
	
	for(int i = 1; i <= order-1; i+=2)
	{
		poles[i] = conj(poles[i-1]);
	}
	
	if(order % 2 != 0)
	{
		poles[order - 1] = complex<double>(-1, 0);
	}
	
	double k;
	complex<double> prod = poles[order-1];
	for (int i = 0; i < order - 1; i++) {
		prod *= poles[i];
	}
	
	k = real(prod);
	
	// Convert analog low-pass filter to state space representation
	MatrixXd a(1,1), b(1,1), c(1,1), d(1,1);
	
	if(order % 2 == 1)
	{
		a(0,0) = real(poles[order-1]);
		b(0,0) = 1;
		c(0,0) = 1;
		d(0,0) = 0;
	}
	
	int i = 0;
	while(i < order -1)
	{
		double* den = polynomial(poles, order);
		double wn = sqrt(abs(poles[i]) * abs(poles[i+1]));
		if(wn == 0 || wn == 1)
			break;
		Matrix2d t;
		t(0,0) = 1;
		t(1,1) = 1/wn;
		Matrix2d mid;
		mid(0, 0) = -den[1];
		mid(0, 1) = -den[2];
		mid(1, 0) = 1;
		mid(1, 1) = 0;
		Vector2d temp1;
		temp1(0) = 1;
		temp1(1) = 0;
		RowVector2d temp2;
		temp2(0) = 0;
		temp2(1) = 1;
		
		Matrix2d invT = t.inverse();
		Matrix2d a1 = invT * mid * t;
		Vector2d b1 = invT * temp1;
		RowVector2d c1 = temp2 * t;
		double d1 = 0;
		
		MatrixXd aFinal(order, order);
		for(int j = 0; j < a.rows(); j++)
		{
			for(int k = 0; k < a.cols(); k++)
			{
				aFinal(j, k) = a(j, k);
			}
		}
		
		temp1 = b1 * c;
		for(int j = 0; j < temp1.rows(); j++)
		{
			for(int k = 0; k < temp1.cols(); k++)
			{
				aFinal(j + a.rows(), k) = temp1(j, k);
			}
		}
		
		for(int j = 0; j < a1.rows(); j++)
		{
			for(int k = 0; k < a1.cols(); k++)
			{
				aFinal(j + a.rows(), k + temp1.cols()) = a1(j, k);
			}
		}
		
		MatrixXd bFinal(order, 1);
		temp1 = b1 * d;
		for(int j = 0; j < b.rows(); j++)
		{
			bFinal(j, 0) = b(j, 0);
		}
		for(int j = 0; j < temp1.rows(); j++)
		{
			bFinal(j + b.rows(), 0) = temp1(j, 0);
		}
		
		MatrixXd cFinal(1, order);
		temp1 = d1 * c;
		for(int j = 0; j < temp1.cols(); j++)
		{
			cFinal(0, j) = temp1(0, j);
		}
		for(int j = 0; j < c1.cols(); j++)
		{
			cFinal(0, j + temp1.cols()) = c1(0, j);
		}
		
		a = aFinal;
		b = bFinal;
		c = cFinal;
		
		i += 2;
		
		free(den);
	}
	c *= k;
	
	if(type == LOWPASS)
	{
		a = w_n * a;
		b = w_n * b;
	}
	else if(type == HIGHPASS)
	{
		a = w_n * a.inverse();
		b = -w_n * (a.inverse() * b);
		c = c * a.inverse();
		d = d - c * a.inverse() * b;
	}
	
	// Bilinear Transform
	double t = 1/fs;
	double r = sqrt(t);
	MatrixXd t1 = MatrixXd::Identity(a.rows(), a.cols()) + a * t/2;
	MatrixXd t2 = MatrixXd::Identity(a.rows(), a.cols()) - a * t/2;
	MatrixXd t2Inv = t2.inverse();
	MatrixXd ad = t2Inv * t1;
	MatrixXd bd = t/r * (t2Inv * b);
	MatrixXd cd = r * c * t2Inv;
	MatrixXd dd = c * t2Inv * b * t /2 + d;
	
	VectorXcd eivals = ad.eigenvalues();
	//cout << "HERE!!!" << endl;
	//cout << eivals << endl;
	double* tem = polynomial(eivals);
	memcpy(den, tem, sizeof(double) * (order + 1));
	
	complex<double> *R = (complex<double>*)malloc(order * sizeof(complex<double>));
	double w = 0;
	
	if(type == LOWPASS)
	{
		for(int i = 0; i < order; i++)
		{
			R[i] = -1;
		}
	}
	else if(type == HIGHPASS)
	{
		for(int i = 0; i < order; i++)
		{
			R[i] = 1;
		}
		w = PI;
	}
	
	double* numTemp = polynomial(R, order);
	
	VectorXcd kern(order + 1);
	for(int i = 0; i <= order; i++)
	{
		kern(i) = complex<double>(cos(w * i), -sin(w * i));
	}
	
	double dotProdNum = 0;
	double dotProdDen = 0;
	for(int i = 0; i <= order; i++)
	{
		dotProdNum += real(kern(i)) * den[i];
		dotProdDen += real(kern(i)) * numTemp[i];
	}	
	
	for(int i = 0; i <= order; i++)
	{
		numTemp[i] *= dotProdNum/dotProdDen;
	}
	
	memcpy(num, numTemp, sizeof(double) * (order + 1));
	free(R);
	free(tem);
}

/*
 * Generates all combinations of poles that occur in the generation of a 
 * polynomial from a list of zeros. Effectively generates a power set of the
 * set {1, 2, ..., n} where n is the number of zeros of the polynomial. This 
 * can be used to generate a polynomial by recognizing that, for each term, 
 * the power of the variable plus the number of zeros involved in the 
 * coefficient must be equal to n, the total number of zeros. For example,
 * for the polynomial (x + 1) * (x + 2) * (x + 3), one term will be 2 * x^2
 * and another term will be 2 * 3 * x.
 *
 *
 * @param length The number of zeros of the polynomial (including algebraic
 * multiplicity).
 *
 * @return Returns a vector of (mulitplicative) combinations of zeros. Each
 * combination is represented as a vector that lists the indices of each 
 * zero to be multiplied for a given term.
 */
vector<vector<int> > IIRFilter::combinations(int numZeros)
{
	if(numZeros == 0)
	{
		vector<int> first;
		vector<vector<int> > blank;
		blank.push_back(first);
		return blank;
	}
	
	vector<vector<int> > subCombos = combinations(numZeros - 1);
	int size = subCombos.size();
	
	for(int i = 0; i < size; i++)
	{
		vector<int> copy = subCombos[i];
		copy.push_back(numZeros - 1);
		subCombos.push_back(copy);
	}
	
	return subCombos;
}

/*
 * Converts a list of complex zero locations for a polynomial into an 
 * array of doubles representing the coefficients of each term of the 
 * polynomial when multiplied out. That is, it converts a polynomial from
 * the form (x + 1)(x + 2) into the form x^2 + 3x + 2. 
 *
 * @param zeros A vector of complex<double>'s indicating the locations of 
 * the zeros of the polynomial. Because this function is used to generate
 * filters for physical systems, it is assumed that complex zeros always 
 * come in complex conjugate pairs.
 *
 * @return An array of doubles indicating the coefficients of the polynomial
 * corresponding to the zeros specified. The indexing convention is such 
 * that the 0th index of the array returned corresponds to the coefficient
 * of the highest power of the variable.
 */
double* IIRFilter::polynomial(VectorXcd zeros)
{
	int numZeros = zeros.rows();
	complex<double>* polyCoeffs = (complex<double>*)(malloc(sizeof(complex<double>) * (numZeros+1)));
	for(int i = 0; i < numZeros+1; i++)
	{
		polyCoeffs[i] = complex<double>(0, 0);
	}
	
	//polyCoeffs[0] = complex<double>(1, 0);
	
	vector<vector<int> > indicies = combinations(numZeros);
	
	for(unsigned int i = 0; i < indicies.size(); i++)
	{
		//cout << "Indicies[" << i << "] = ";
		int curSize = indicies[i].size();
		
		complex<double> temp (1, 0);
		for(int j = 0; j < curSize; j++){
			//cout << indicies[i][j] << ", ";
			temp *= (-zeros[indicies[i][j]]);
		}
		//cout << endl;
		
		polyCoeffs[curSize] += temp;
	}
	//cout << "size " << indicies.size() << endl;
	double *toReturn = (double*)(malloc(sizeof(double) * (numZeros + 1)));
	for(int i = 0; i < numZeros + 1; i++)
	{
		toReturn[i] = real(polyCoeffs[i]);
	}
	free(polyCoeffs);
	return toReturn;
}

/*
 * Converts a list of complex zero locations for a polynomial into an 
 * array of doubles representing the coefficients of each term of the 
 * polynomial when multiplied out. That is, it converts a polynomial from
 * the form (x + 1)(x + 2) into the form x^2 + 3x + 2. 
 *
 * @param zeros An array of complex<double>'s indicating the locations of 
 * the zeros of the polynomial. Because this function is used to generate
 * filters for physical systems, it is assumed that complex zeros always 
 * come in complex conjugate pairs.
 *
 * @param numZeros An integer representing the number of zeros in the 
 * polynomial
 *
 * @return An array of doubles indicating the coefficients of the polynomial
 * corresponding to the zeros specified. The indexing convention is such 
 * that the 0th index of the array returned corresponds to the coefficient
 * of the highest power of the variable.
 */
double* IIRFilter::polynomial(complex<double> *zeros, int numZeros)
{
	complex<double>* polyCoeffs = (complex<double>*)(malloc(sizeof(complex<double>) * (numZeros+1)));
	for(int i = 0; i < numZeros+1; i++)
	{
		polyCoeffs[i] = complex<double>(0, 0);
	}
	
	//polyCoeffs[0] = complex<double>(1, 0);
	
	vector<vector<int> > indicies = combinations(numZeros);
	
	for(unsigned int i = 0; i < indicies.size(); i++)
	{
		int curSize = indicies[i].size();
		
		complex<double> temp (1, 0);
		for(int j = 0; j < curSize; j++)
			temp *= (-zeros[indicies[i][j]]);
		
		polyCoeffs[curSize] += temp;
	}
	
	double *toReturn = (double*)(malloc(sizeof(double) * (numZeros + 1)));
	for(int i = 0; i < numZeros + 1; i++)
	{
		toReturn[i] = real(polyCoeffs[i]);
	}
	free(polyCoeffs);
	return toReturn;
}

/*
 * Computes the coefficients of the numerator and denominator polynomials 
 * for the discrete-time (z-domain) transfer function for a first order 
 * shelving filter.
 *
 * @param b Vector of doubles representing the coefficients of the numerator
 * polynomial for the discrete-time (z-domain) transfer function
 *
 * @param a Vector of doubles representing the coefficients of the 
 * denominator polynomial for the discrete-time (z-domain) transfer function
 *
 * @param tone A double between 0 and 100 that changes the output tone of 
 * the shelving filter (0 corresponds to a more muddy, bassy sound whereas
 * 100 corresponds to a more trebly sound).
 *
 * @param w_c The cutoff frequency of the shelving filter
 *
 * @param w_s The sampling frequency of the shelving filter
 *
 * @todo
 * Add support for higher order shelving filters
 */
void IIRFilter::shelvingFilter(vector<double> &b, vector<double> &a, double tone,
										double w_c, double w_s)
{
	tone = (tone / 100 * 40) - 20;
	
	double R = w_c / w_s;
	double K = tan(PI * R);
	double V0 = pow(10, tone / 20);
	double sqrt2 = sqrt(2);
	
	b.push_back((V0 + sqrt(V0) * sqrt2 * K + K * K) / (1 + sqrt2 * K + K * K));
	b.push_back((2 * ( K * K - V0 )) / (1 + sqrt2 * K + K * K));
	b.push_back((V0 - sqrt(V0) * sqrt2 * K + K * K) / (1 + sqrt2 * K + K * K));
	a.push_back(1);
	a.push_back((2 * (K * K - 1)) / (1 + sqrt2 * K + K * K));
	a.push_back((1 - sqrt2 * K + K * K) / (1 + sqrt2 * K + K * K));
	/*
	for(int i = 0;  i<3; i++)
		cout << "B[" << i << "]: " << b[i] << endl;
	for(int i = 0; i < 3; i++)
		cout << "A[" << i << "]: " << a[i] << endl;*/
}


